//////////////////////////////
// serial buffer loading
const int _iMaxSerialBuffer = 512;
int _iSerialBufferStart=0, _iSerialBufferLen=0, _iSerialBufferIndex=0;
char _SerialBuffer[_iMaxSerialBuffer];
boolean readSerialString (String &cmd) {
  int sb;
  while (Serial.available())
  {
    sb = Serial.read();
    if(sb=='#') // start cmd ?
    {
      _iSerialBufferStart = _iSerialBufferIndex+1;
    }
    else if(sb=='$' && _iSerialBufferStart>=0) // end cmd ?
    {
      _iSerialBufferLen = _iSerialBufferIndex;
      _SerialBuffer[_iSerialBufferLen] = '\0';
      cmd = &_SerialBuffer[_iSerialBufferStart];
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return true;
    }
    if(_iSerialBufferIndex>=_iMaxSerialBuffer)
    {
      _iSerialBufferIndex = 0;
      _iSerialBufferStart = -1;
      return false;
    }
    _SerialBuffer[_iSerialBufferIndex] = sb;
    _iSerialBufferIndex++;
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, const int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}
int load_int_nosigne(String &s, const int start, int &iRet)
{
  int iLen = start;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";

  for(int i=0; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

void check_commands(void)
{
  String line="";
  if(!readSerialString(line))
    return;
  if(line.length()>0)
  {
    // debug only : send_message(line);

    switch(line[0])
    {
    case 'v': // (V)ersion
      {
        // #v$
        send_version();
      }
      break;      
    case 'g': // settin(G)
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 'b':  // (B)ody setting
              // #gab$
              send_setting_body();        
              break;
            case 't':  // (T)urrel setting
              // #gat$
              send_setting_turret();
              break;
            case 'r':  // (R)ay setting
              // #gar$
              send_setting_ray();
              break;
            default: // general setting
              // #ga-$
              send_setting();
              break;
            }
          }
          break;
        case 's': // (S)et
          {
            switch(line[2])
            {
            case 'r':  // (R)ay setting
              {
                switch(line[3])
                {
                case 'c':  // ray (C)ount setting
                  {
                    // #gsrc5$
                    int bc=_beam_count;
                    int lbs = load_int_nosigne(line, 4, bc);
                    _beam_count = bc;
                    send_setting_ray();
                  }
                  break;
                case 'd':  // ray (D)elay setting
                  {
                    // #gsrd5$
                    int br=_beam_delay;
                    int lbs = load_int_nosigne(line, 4, br);
                    _beam_delay = br;
                    send_setting_ray();
                  }
                  break;
                case 's':  // ray (S)ensors setting
                  {
                    // #gsrs3$
                    int bs=_beam_sensors;
                    int lbs = load_int_nosigne(line, 4, bs);
                    _beam_sensors = bs;
                    send_setting_ray();
                  }
                  break;
                case 'p':  // ray (P)recision setting
                  {
                    // #gsrs3$
                    int bp=_beam_precision*100;
                    int lbs = load_int_nosigne(line, 4, bp);
                    _beam_precision = (float)bp/100.0f;
                    send_setting_ray();
                  }
                  break;
                }
              }
              break;
            }
          }
          break;
        }
      } 
      break;            
    case 's': // (S)tatus    
      {
        switch(line[1])
        {
        case 'a': // (A)sk
          {
            switch(line[2])
            {
            case 's':  // (S)ensors
              // #sas$
              send_status_hitsensors();
              break;
            case 't':  // (T)urrel
              // #sat$
              send_status_turret();
              break;
            case 'b':  // (B)ody
              // #sab$
              send_status_body();
              break;
            }
          }
          break;
        case 's': // (S)et
          {
            switch(line[2])
            {
            case 's': // (S)end status automatic
              {
                // #sss0$/#sss1$
                char param = line[3];
                if(param=='0') _send_status = false;
                else if(param=='1') _send_status = true;
                send_setting();
              }
              break;
            }
          }
          break;
        }
      }
      break;
    case 'r': // (R)eset
      {
        switch(line[1])
        {
        case 'b': // (B)ody
          // #rb$
          motor_stop();
          break;
        case 't': // (T) turret
          // #rt$
          reset_turret_position(100);
          if(!_send_status) send_status_turret();
          break;
        case 'g': // tar(G)et
          // #rg$
          target_stop();
          break;
        default: // All !
          // #r-$
          target_stop();
          motor_stop(); 
          reset_turret_position(100);
          if(!_send_status) send_status_turret();
          break;
        }
      }
      break;      
    case 'b': // (B)ody stuff
      {
        switch(line[1])
        {
        case 'm': // 'M'oving
          {
            // #bm11+25+25$
            boolean bL = (line[2]=='0')?false:true;
            boolean bR = (line[3]=='0')?false:true;
            int iL=0, iR=0;
            int endl = load_int(line, 4, iL);
            load_int(line, endl, iR);
            int iPwnL = map_power_motor(iL);
            int iPwnR = map_power_motor(iR);
            motor_action(iPwnL, iPwnR, bL, bR);
          }
          break;
        case 't': // 'T'argetting
          {
            switch(line[2])
            {
            case 'm': // 'M'oving
              {
                // #btm1+25+15$
                boolean bF = (line[3]=='0')?false:true;
                int iP=0, iLen=0;
                int endp = load_int(line, 4, iP);
                load_int(line, endp, iLen);
                move_to_target(bF, iP, iLen);
              }
              break;
            case 'r': // 'R'otating
              {
                // #btr+25+250$
                int iP=0, iBear=0;
                int endp = load_int(line, 3, iP);
                load_int(line, endp, iBear);
                rotate_to_target(iP, iBear);
              }
              break;
            }
          }
          break;
        }
      }
      break;      
    case 't': // (T)urrel stuff
      {
        switch(line[1])
        {
        case 'p': // set 'P'osition
          {
            // #tp+90+90$
            int iLow=0, iHigh=0;
            int endl = load_int(line, 2, iLow);
            load_int(line, endl, iHigh);
            set_turret_position(iLow, iHigh);
            if(!_send_status) send_status_turret();
          }
          break;
        case 's': // 'S'can mode !
          {
            switch(line[2])
            {
            case 'p':
              {
                // #tsp+90+90$
                int iLow=0, iHigh=0;
                int endl = load_int(line, 3, iLow);
                load_int(line, endl, iHigh);
                scan_point(iLow, iHigh);
                if(!_send_status) send_status_turret();
              }
              break;
            case 'l':
              {
                // #tsl+85+95+1+75$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHigh=0;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                load_int(line, endl, iHigh);
                scan_line(iLowBegin, iLowEnd, iLowStep, iHigh);
                if(!_send_status) send_status_turret();
              }
              break;
            case 'z':
              {
                // #tsz+85+95+1+85+95+1$
                int iLowBegin=0, iLowEnd=0, iLowStep=0, iHighBegin=0, iHighEnd=0, iHighStep;
                int endl = load_int(line, 3, iLowBegin);
                endl = load_int(line, endl, iLowEnd);
                endl = load_int(line, endl, iLowStep);                
                endl = load_int(line, endl, iHighBegin);
                endl = load_int(line, endl, iHighEnd);                
                load_int(line, endl, iHighStep);
                scan_zone_z(iLowBegin, iLowEnd, iLowStep, iHighBegin, iHighEnd, iHighStep);
                if(!_send_status) send_status_turret();
              }
              break;
            }
          }
          break;
        }
      }
      break;      
    default:
      if(_send_invalid_cmd)
      {
        String sError = "invalid cmd '" + line + "'";
        send_error(sError);
      }
      break;
    }
  }
}


